#include "drv_servo.h"
#include "drv_systick.h"
/**************************************
函数名：drv_pwm_init
函数功能：pwm初始化
输入参数：无
输出参数：无
其它： 定时20ms中断一次
		分频=84；装载值=5000；
****************************************/
void drv_pwm_init(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	GPIO_InitTypeDef GPIO_InitStruct;
	TIM_OCInitTypeDef TIM_OCInitStruct;
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);//开启GPIOA时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);//开启TIM1时钟

	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;//复用模式
	GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;//推挽输出
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9|GPIO_Pin_10|GPIO_Pin_11;//引脚为8、9、10、11
	GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_25MHz;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource8,  GPIO_AF_TIM1);  // PA8 -> TIM1_CH1
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource9,  GPIO_AF_TIM1);  // PA9 -> TIM1_CH2
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource10, GPIO_AF_TIM1);  // PA10 -> TIM1_CH3
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_TIM1);  // PA11 -> TIM1_CH4
	
	TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//时钟分割，输入才用
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//选择计数方式为向上计数
	TIM_TimeBaseInitStruct.TIM_Period = 20000-1;//装载值(就是数多少次)
	TIM_TimeBaseInitStruct.TIM_Prescaler = 168-1;//预分频值
	TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);//用上面配置的参数初始化定时器1

	TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;//输出模式为PWM1
	TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;//输出极性为高
	TIM_OCInitStruct.TIM_OutputState = ENABLE;//使能输出
	TIM_OCInitStruct.TIM_Pulse = 0;//比较初值


	TIM_OC1Init(TIM1, &TIM_OCInitStruct);  // 通道 1（PA8）
    TIM_OC2Init(TIM1, &TIM_OCInitStruct);  // 通道 2（PA9）
    TIM_OC3Init(TIM1, &TIM_OCInitStruct);  // 通道 3（PA10）
    TIM_OC4Init(TIM1, &TIM_OCInitStruct);  // 通道 4（PA11）

    TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);  // 通道 1 比较影子寄存器
    TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);  // 通道 2
    TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);  // 通道 3
    TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);  // 通道 4
    
	TIM_ARRPreloadConfig(TIM1, ENABLE);          // 自动重装载影子寄存器
	TIM_CtrlPWMOutputs(TIM1,ENABLE);//使能输出PWM输出
	TIM_Cmd(TIM1, ENABLE);//使能定时器 1
}

// angle: 0~180 度，对应 0.5ms~2.5ms 高电平
void sg90_set_angle(uint8_t angle, uint8_t channel)
{
    // 角度映射：0度对应500，180度对应2500，先算比例
    uint16_t pulse = 500 + (angle * (2500 - 500) / 180);  
	//uint16_t pulse = 1000 + (angle * (2000 - 1000) / 180);  // 1ms~2ms范围
    switch(channel)//正面是靠近蜂鸣器那一边
    {
        case 1:
            TIM_SetCompare1(TIM1, pulse);//左前
            break;
        case 2:
            TIM_SetCompare2(TIM1, pulse);//左后
            break;
        case 3:
            TIM_SetCompare3(TIM1, pulse);//右前
            break;
        case 4:
            TIM_SetCompare4(TIM1, pulse);//右后
            break;
        default:
            break;
    }
}


// 站立函数（优化后）
void Stand_up(void)
{
    int angle;
    // 1、4号舵机正向转动，2、3号舵机反向转动
    for(angle = 0; angle <= 90; angle += 10)
    {
        sg90_set_angle(angle, 1);           // 1号正向
        sg90_set_angle(angle, 2);           // 2号正向
			sg90_set_angle(180 - angle, 3);     // 3号反向（互补角度）
        sg90_set_angle(180 - angle, 4);     // 4号反向（互补角度）
        drv_systick_ms(50);
    }
    drv_systick_ms(1000);
}

// 趴下函数（优化后）
void Get_down(void)
{
    int angle;
    // 1、4号舵机反向转动，2、3号舵机正向转动
    for(angle = 90; angle >= 0; angle -= 10)
    {
        sg90_set_angle(angle, 1);           // 1号正向
        sg90_set_angle(angle, 2);           // 2号正向
		sg90_set_angle(180 - angle, 3);     // 3号反向
        sg90_set_angle(180 - angle, 4);     // 4号反向
        drv_systick_ms(50);
    }
    drv_systick_ms(1000);
}

//// 坐下函数（优化后）
//void Sit_down(void)
//{
//    int angle;
//    // 1、4号舵机反向转动，2、3号舵机正向转动
//    for(angle = 90; angle >= 0; angle -= 10)
//    {
//        //sg90_set_angle(angle, 1);           // 1号正向
//        sg90_set_angle(angle, 2);           // 2号正向
//		//sg90_set_angle(180 - angle, 3);     // 3号反向
//        sg90_set_angle(180 - angle, 4);     // 4号反向
//        drv_systick_ms(50);
//    }
//    drv_systick_ms(1000);
//}
// 坐下函数（简化版，确保2号30°、4号反向30°）
void Sit_down(void)
{
    int angle;
    // 2号从85°→30°，4号从80°→150°（180-30）
    // 共11步，每步5°，同步到位
    for(angle = 0; angle <= 55; angle += 5)
    {
        sg90_set_angle(100, 1);               // 1号固定
        sg90_set_angle(85 - angle, 2);        // 2号：85°→30°（30°基准）
        sg90_set_angle(90, 3);                // 3号固定
        sg90_set_angle(80 + angle + 3, 4);   // 4号：80°→150°（+15补偿差值）
        drv_systick_ms(50);
    }
    drv_systick_ms(1000);
}

// 后退函数（根据修改优化版）
void Move_Backward(void)
{
    int step;
    for(step = 0; step < 2; step++)
    {
        // 第一步：3、4号压低（保持3号与4号的10°差）
        int angle;
        for(angle = 90; angle >= 50; angle -= 5)
        {
            // 1、2号按原比例抬升，保持幅度不变
            sg90_set_angle(100 + (100-angle)/2, 1);  // 100°→120°
            sg90_set_angle(85 + (100-angle)/2, 2);   // 85°→105°
            // 3号从80°→40°（与4号保持同步差），4号从70°→30°
            sg90_set_angle(angle - 10, 3);          // 3号核心修改点
            sg90_set_angle(angle - 20, 4);          // 保持与3号10°差
            drv_systick_ms(30);
        }
        drv_systick_ms(300);
        
        // 第二步：3、4号回撤（延续10°差，增大整体幅度）
        for(angle = 50; angle <= 130; angle += 5)
        {
            // 1、2号按原比例回落
            sg90_set_angle(120 - (angle-50)/1.6, 1);  // 120°→70°
            sg90_set_angle(105 - (angle-50)/1.6, 2);   // 105°→55°
            // 3号从40°→120°，4号从30°→110°（保持差）
            sg90_set_angle(angle - 10, 3);            // 维持你的修改
            sg90_set_angle(angle - 20, 4);            // 同步偏移
            drv_systick_ms(30);
        }
        drv_systick_ms(300);
        
        // 第三步：回归站立位（精准确定位初始角度）
        for(angle = 130; angle >= 90; angle -= 5)
        {
            // 1、2号精准回归100°、85°
            sg90_set_angle(70 + (130-angle)/1.6, 1);  // 70°→100°
            sg90_set_angle(55 + (130-angle)/1.6, 2);   // 55°→85°
            // 3号回归90°，4号回归80°
            sg90_set_angle(angle - 10, 3);            // 120°→80°→90°修正
            sg90_set_angle(angle - 20, 4);            // 110°→70°→80°修正
            drv_systick_ms(30);
        }
        drv_systick_ms(200);
    }
}



// 前进函数（修复中间趴下问题）
void Move_Forward(void)
{
    int step;
    for(step = 0; step < 2; step++)
    {
        // 第一步：1、2号轻微压低（从站立位100°→80°），避免过度低头
        int angle;
        for(angle = 100; angle >= 80; angle -= 5)  // 角度范围缩小（100°→80°）
        {
            int angle2 = angle - 15;  // 2号同步：85°→65°（仍高于低位）
            // 3号、4号抬起角度减小（避免过度抬头）
            int angle3 = 180 - (angle - 20);  // 从90°→110°（合理范围）
            int angle4 = 180 - (angle - 20);  // 从80°→100°（合理范围）
            
            sg90_set_angle(angle, 1);    // 左前：100°→80°（轻微压低）
            sg90_set_angle(angle2, 2);   // 左后：85°→65°
            sg90_set_angle(angle3, 3);   // 右前：90°→110°（轻微抬起）
            sg90_set_angle(angle4, 4);   // 右后：80°→100°（轻微抬起）
            drv_systick_ms(30);
        }
        drv_systick_ms(300);
        
        // 第二步：1、2号轻微回撤（80°→120°），3、4号轻微推进
        for(angle = 80; angle <= 120; angle += 5)  // 最大角度120°（低于原140°）
        {
            int angle2 = angle - 15;  // 2号同步：65°→105°
            // 3号、4号反向角度：180-120=60°（高于原40°，远离低位）
            int angle3 = 180 - angle;  
            int angle4 = 180 - angle;  
            
            sg90_set_angle(angle, 1);    // 左前：80°→120°（适度抬起）
            sg90_set_angle(angle2, 2);   // 左后：65°→105°
            sg90_set_angle(angle3, 3);   // 右前：110°→60°（适度推进）
            sg90_set_angle(angle4, 4);   // 右后：100°→60°（适度推进）
            drv_systick_ms(30);
        }
        drv_systick_ms(300);
        
        // 第三步：回归站立位（严格回到100°、85°、90°、80°）
        for(angle = 120; angle >= 100; angle -= 5)  
        {
            int angle2 = angle - 15;  // 2号回归85°（105°→85°）
            int angle3 = 180 - angle; // 3号回归90°（60°→90°）
            int angle4 = 180 - angle; // 4号回归80°（60°→80°）
            
            sg90_set_angle(angle, 1);    // 左前：120°→100°
            sg90_set_angle(angle2, 2);   // 左后：105°→85°
            sg90_set_angle(angle3, 3);   // 右前：60°→90°
            sg90_set_angle(angle4, 4);   // 右后：60°→80°
            drv_systick_ms(30);
        }
        drv_systick_ms(200);
    }
}

// 简洁舵机初始化函数：直接设置为站立姿势
void Servo_Init(void)
{	
    // 直接设置所有舵机到站立位置（90度）
    // 保持1、4号正向，2、3号反向的逻辑
    sg90_set_angle(100, 1);           // 1号舵机站立角度
    sg90_set_angle(85, 2);           // 2号舵机站立角度
		sg90_set_angle(180 - 90, 3);     // 3号舵机反向站立角度
    sg90_set_angle(180 - 100, 4);     // 4号舵机反向站立角度
    
    drv_systick_ms(1000);  // 等待所有舵机到位
}